#include "MySensors.h"
/*-------------------------------------------------------*/
// Accelerometer functions
/*-------------------------------------------------------*/

// ADXL345
#ifdef ADXL345

void ADXL345_Init(uint8_t adr)
{
	//initialize
	status = ADXL345_OK;
	error_code = ADXL345_NO_ERROR;

	gains[0] = 0.00376390;
	gains[1] = 0.00376009;
	gains[2] = 0.00349265;

	ADXL345_address = adr << 1;

	ADXL345_PowerOn();
	ADXL345_SetJustifyBit(0);	// false
	ADXL345_SetFullResBit(1);	// true
	ADXL345_SetRangeSetting(2);	// 2: 2g, 4: 4g, 8: 8g, 16: 16g 
}

void ADXL345_getValues(float * x, float * y, float * z)
{
	uint8_t _buff[6];
	ADXL345_ReadFrom(ADXL345_DATAX0, 6, _buff); //read the acceleration data from the ADXL345
	// each axis reading comes in 10 bit resolution, ie 2 bytes.  Least Significat Byte first!!
	// thus we are converting both bytes in to one int
	*x = (((int)_buff[1]) << 8) | _buff[0];  
	*y = (((int)_buff[3]) << 8) | _buff[2];
	*z = (((int)_buff[5]) << 8) | _buff[4];
}

void ADXL345_PowerOn()
{
	ADXL345_WriteTo(ADXL345_POWER_CTL, 8); //power register
}

// Writes val to address register on device
void ADXL345_WriteTo(unsigned char address, unsigned char val)
{
	i2c_start(ADXL345_address + I2C_WRITE);
	i2c_write(address);
	i2c_write(val);
	i2c_stop();
}

// Reads num bytes starting from address register on device in to _buff array
void ADXL345_ReadFrom(unsigned char address, unsigned char num, unsigned char _buff[])
{
	unsigned char i = 0;
	i2c_start(ADXL345_address + I2C_WRITE);	// start transmission to device
	i2c_write(address);						// sends address to read from
	i2c_stop();								// end transmission
	i2c_start(ADXL345_address + I2C_READ);	// start transmission to device
	while (i < num - 1)
	{
		_buff[i++] = i2c_readAck();
	}
	_buff[i] = i2c_readNak();
	i2c_stop();
}

// Sets the range setting, possible values are: 2, 4, 8, 16
void ADXL345_SetRangeSetting(int val)
{
	unsigned char _s, _b;
	switch (val)
	{
		case 2:
			_s = 0b00000000;
			break;
		case 4:
			_s = 0b00000001;
			break;
		case 8:
			_s = 0b00000010;
			break;
		case 16:
			_s = 0b00000011;
			break;
		default:
			_s = 0b00000000;
	}
	ADXL345_ReadFrom(ADXL345_DATA_FORMAT, 1, &_b);
	_s |= (_b & 0b11101100);
	ADXL345_WriteTo(ADXL345_DATA_FORMAT, _s);
}

void ADXL345_SetRegisterBit(unsigned char regAdress, unsigned char bitPos, unsigned char state)
{
	unsigned char _b;
	ADXL345_ReadFrom(regAdress, 1, &_b);
	if (state)
	{
		_b |= (1 << bitPos);  // forces nth bit of _b to be 1.  all other bits left alone.
	} else
	{
		_b &= ~(1 << bitPos); // forces nth bit of _b to be 0.  all other bits left alone.
	}
	ADXL345_WriteTo(regAdress, _b);  
}


// Sets the JUSTIFY bit
// if sets to 1 selects the left justified mode
// if sets to 0 selects right justified mode with sign extension
void ADXL345_SetJustifyBit(unsigned char justifyBit)
{
	ADXL345_SetRegisterBit(ADXL345_DATA_FORMAT, 2, justifyBit);
}

// Sets the FULL_RES bit
// if set to 1, the device is in full resolution mode, where the output resolution increases with the
//   g range set by the range bits to maintain a 4mg/LSB scal factor
// if set to 0, the device is in 10-bit mode, and the range buts determine the maximum g range
//   and scale factor
void ADXL345_SetFullResBit(unsigned char fullResBit)
{
	ADXL345_SetRegisterBit(ADXL345_DATA_FORMAT, 3, fullResBit);
}

#endif

// BMA180
#ifdef BMA180

void BMA180_Init(uint8_t adr)
{
	BMA180_setAddress(adr);
	BMA180_SoftReset();
	BMA180_enableWrite();
	BMA180_SetFilter(F10HZ);
	BMA180_setGSensitivty(G15);
	BMA180_SetSMPSkip();
	BMA180_SetISRMode();
	BMA180_disableWrite();
}

void BMA180_setAddress(unsigned char adr)
{
	BMA180_address = adr << 1;
}

void BMA180_getValues(float * x, float * y, float * z)
{
	int tmp;
	i2c_start(BMA180_address + I2C_WRITE);
	i2c_write(0x02);
	i2c_start(BMA180_address + I2C_READ);
	unsigned char lsb = i2c_readAck() >> 2;
	unsigned char msb = i2c_readAck();
	tmp = (msb << 6) + lsb;
	if ((tmp) & 0x2000) (tmp) |= 0xc000; // set full 2 complement for neg values
	*x = (float)tmp;
	lsb = i2c_readAck() >> 2;
	msb = i2c_readAck();
	tmp = (msb << 6) + lsb;
	if ((tmp) & 0x2000) (tmp) |= 0xc000;
	*y = (float)tmp;
	lsb = i2c_readAck() >> 2;
	msb = i2c_readNak();
	tmp = (msb << 6) + lsb;
	if ((tmp) & 0x2000) (tmp) |= 0xc000;
	*z = (float)tmp;
	i2c_stop();
}

unsigned char BMA180_getRegValue(unsigned char adr)
{
	unsigned char val;
	i2c_start(BMA180_address + I2C_WRITE);
	i2c_write(adr);
	i2c_stop();
	i2c_start(BMA180_address + I2C_READ);
	val = i2c_readNak();
	i2c_stop();
	return val;
}

void BMA180_setRegValue(unsigned char regAdr, unsigned char val, unsigned char maskPreserve)
{
    unsigned char preserve = BMA180_getRegValue(regAdr);
    unsigned char orgval = preserve & maskPreserve;
	i2c_start(BMA180_address + I2C_WRITE);
	i2c_write(regAdr);
	i2c_write(orgval | val);
	i2c_stop();
}

void BMA180_setGSensitivty(GSENSITIVITY maxg) //1, 1.5 2 3 4 8 16
{
	BMA180_setRegValue(0x35, maxg << 1, 0xF1);
}

void BMA180_SetFilter(FILTER f) // 10,20,40,75,150,300,600,1200, HP 1HZ,BP 0.2-300, higher values not authorized
{
	BMA180_setRegValue(0x20, f << 4, 0x0F);
}

void BMA180_SetISRMode() // you must provide a ISR function on the pin selected (pin 2 or 3,. so INT0 or INT1)
{
	BMA180_setRegValue(0x21, 2, 0xFD);
}

void BMA180_SoftReset() // all values will be default
{
	BMA180_setRegValue(0x10, 0xB6, 0);
	_delay_ms(100);
}

void BMA180_SetSMPSkip()
{
	BMA180_setRegValue(0x35, 1, 0xFE);
}

void BMA180_enableWrite()
{	//ctrl_reg1 register set ee_w bit to enable writing to regs.
	BMA180_setRegValue(0x0D, 0x10, ~0x10);
	_delay_ms(10);
}

void BMA180_disableWrite()
{
	BMA180_setRegValue(0x0D, 0x0, ~0x10);
	_delay_ms(10);
}

#endif

/*-------------------------------------------------------*/
// Gyrometer functions
/*-------------------------------------------------------*/
#if defined(ITG3200) || defined(MPU3050)
void ITG3200_Init(unsigned char adr)
{
	ITG3200_setGains(1.0, 1.0, 1.0);
	ITG3200_setOffsets(0.0, 0.0, 0.0);
	ITG3200_setRevPolarity(0, 0, 0);
	ITG3200_setAddress(adr);
	ITG3200_setSampleRateDiv(NOSRDIVIDER);
	ITG3200_setFSRange(RANGE2000);
	ITG3200_setFilterBW(BW256_SR8);
	ITG3200_setClockSource(PLL_XGYRO_REF);
	ITG3200_setITGReady(1);
	ITG3200_setRawDataReady(1);
	_delay_ms(70);			// GYROSTART_UP_DELAY
	ITG3200_zeroCalibrate(64, 5);
}

void ITG3200_setOffsets(int _Xoffset, int _Yoffset, int _Zoffset)
{
	offsets[0] = _Xoffset;
	offsets[1] = _Yoffset;
	offsets[2] = _Zoffset;
}

void ITG3200_setRevPolarity(uint8_t _Xpol, uint8_t _Ypol, uint8_t _Zpol)
{
	polarities[0] = _Xpol ? -1 : 1;
	polarities[1] = _Ypol ? -1 : 1;
	polarities[2] = _Zpol ? -1 : 1;
}

void ITG3200_setGains(float _Xgain, float _Ygain, float _Zgain)
{
	gains[0] = _Xgain;
	gains[1] = _Ygain;
	gains[2] = _Zgain;
}

void ITG3200_setAddress(unsigned char adr)
{
	ITG3200_address = adr << 1;
}

void ITG3200_setSampleRateDiv(uint8_t _SampleRate)
{
	ITG3200_writemem(SMPLRT_DIV, _SampleRate);
}

void ITG3200_setFSRange(uint8_t _Range)
{
	ITG3200_readmem(DLPF_FS, 1, &_buff[0]);
	ITG3200_writemem(DLPF_FS, ((_buff[0] & ~DLPFFS_FS_SEL) | (_Range << 3))); 
}

void ITG3200_setFilterBW(uint8_t _BW)
{
	ITG3200_readmem(DLPF_FS, 1, &_buff[0]);
	ITG3200_writemem(DLPF_FS, ((_buff[0] & ~DLPFFS_DLPF_CFG) | _BW)); 
}

void ITG3200_setClockSource(uint8_t _CLKsource)
{
	ITG3200_readmem(PWR_MGM, 1, &_buff[0]);
	ITG3200_writemem(PWR_MGM, ((_buff[0] & ~PWRMGM_CLK_SEL) | _CLKsource)); 
}

void ITG3200_setITGReady(uint8_t _State)
{
	ITG3200_readmem(INT_CFG, 1, &_buff[0]);
	ITG3200_writemem(INT_CFG, ((_buff[0] & ~INTCFG_ITG_RDY_EN) | _State << 2)); 
}

void ITG3200_setRawDataReady(uint8_t _State)
{
	ITG3200_readmem(INT_CFG, 1, &_buff[0]);
	ITG3200_writemem(INT_CFG, ((_buff[0] & ~INTCFG_RAW_RDY_EN) | _State)); 
}

void ITG3200_zeroCalibrate(unsigned char totSamples, unsigned char sampleDelayMS)
{
	int xyz[3];
	float tmpOffsets[] = {0, 0, 0};
	for (unsigned char i = 0; i < totSamples; i++)
	{
		_delay_ms(sampleDelayMS);
		ITG3200_readGyroRaw1(xyz);
		tmpOffsets[0] += xyz[0];
		tmpOffsets[1] += xyz[1];
		tmpOffsets[2] += xyz[2];
	}
	ITG3200_setOffsets(-tmpOffsets[0] / totSamples, -tmpOffsets[1] / totSamples, -tmpOffsets[2] / totSamples);
}

void ITG3200_writemem(uint8_t _addr, uint8_t _val)
{
	i2c_start(ITG3200_address + I2C_WRITE);		// start transmission to device 
	i2c_write(_addr);							// send register address
	i2c_write(_val);							// send value to write
	i2c_stop();									// end transmission
}

void ITG3200_readmem(uint8_t _addr, uint8_t _nbytes, uint8_t __buff[])
{
	i2c_start(ITG3200_address + I2C_WRITE);		// start transmission to device 
	i2c_write(_addr);							// sends register address to read from
	i2c_stop();									// end transmission
	i2c_start(ITG3200_address + I2C_READ);		// start transmission to device 
	uint8_t i = 0;
	while (i < _nbytes - 1)
	{
    	__buff[i++] = i2c_readAck();			// receive DATA
	}
	__buff[i] = i2c_readNak();					// receive DATA
	i2c_stop();									// end transmission
}

void ITG3200_readGyroRaw(int *_GyroX, int *_GyroY, int *_GyroZ)
{
	ITG3200_readmem(GYRO_XOUT, 6, _buff);
	*_GyroX = ((_buff[0] << 8) | _buff[1]);
	*_GyroY = ((_buff[2] << 8) | _buff[3]);
	*_GyroZ = ((_buff[4] << 8) | _buff[5]);
/*	*_GyroX = (uint16_t)&_buff[0];
	*_GyroY = (uint16_t)&_buff[2];
	*_GyroZ = (uint16_t)&_buff[4];*/
/*	union UData16 u16;
	u16.LH[0] = _buff[0];
	u16.LH[1] = _buff[1];
	*_GyroX = u16.Data;
	u16.LH[0] = _buff[0];
	u16.LH[1] = _buff[1];
	*_GyroX = u16.Data;
	u16.LH[0] = _buff[0];
	u16.LH[1] = _buff[1];
	*_GyroX = u16.Data;*/
}

void ITG3200_readGyroRaw1(int *_GyroXYZ)
{
	ITG3200_readGyroRaw(_GyroXYZ, _GyroXYZ+1, _GyroXYZ+2);
}

void ITG3200_readGyroRawCal(int *_GyroX, int *_GyroY, int *_GyroZ)
{
	ITG3200_readGyroRaw(_GyroX, _GyroY, _GyroZ);
	*_GyroX += offsets[0];
	*_GyroY += offsets[1];
	*_GyroZ += offsets[2];
}

void ITG3200_getValues(float *_GyroX, float *_GyroY, float *_GyroZ)
{
	int x, y, z;
	ITG3200_readGyroRawCal(&x, &y, &z);	// x,y,z will contain calibrated integer values from the sensor
	#ifdef ITG3200
	float c = 1.2141420883E-3;			// M_PI / 180 / 14.375 = 1.2141420883E-3;	=> doi voi ITG3200
	#endif
	#ifdef MPU3050
	float c = 1.0642251537E-3;			// M_PI / 180 / 16.4 = 1.0642251537E-3;		=> doi voi MPU3050, MPU6050
	#endif
	*_GyroX =  x * polarities[0] * gains[0] * c;	// rad/sec
	*_GyroY =  y * polarities[1] * gains[1] * c;
	*_GyroZ =  z * polarities[2] * gains[2] * c;
}
#endif

#ifdef MPU6050

void MPU6050_Init(uint8_t address)
{
	MPU6050_address = address << 1;
/*	MPU6050_setClockSource(MPU6050_CLOCK_PLL_XGYRO);
	MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
	MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
	MPU6050_setSleepEnabled(0);*/

	I2Cdev_writeByte(MPU6050_address, 0x6B, 0x80);             //PWR_MGMT_1    -- DEVICE_RESET 1
	_delay_ms(5);
	I2Cdev_writeByte(MPU6050_address, 0x19, 0x00);             //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
	I2Cdev_writeByte(MPU6050_address, 0x1A, 0); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
	I2Cdev_writeByte(MPU6050_address, 0x6B, 0x03);             //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
	I2Cdev_writeByte(MPU6050_address, 0x1B, 0x18);             //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec
	I2Cdev_writeByte(MPU6050_address, 0x1C, 0x10);             //ACCEL_CONFIG  -- AFS_SEL=2 (Full Scale = +/-8G)  ; ACCELL_HPF=0   //note something is wrong in the spec.

	#if defined(AUX_MAG)
	// enable I2C bypass for AUX I2C
	I2Cdev_writeByte(MPU6050_address, 0x6A, 0x00);             //USER_CTRL     -- DMP_EN=0 ; FIFO_EN=0 ; I2C_MST_EN=0 (I2C bypass mode) ; I2C_IF_DIS=0 ; FIFO_RESET=0 ; I2C_MST_RESET=0 ; SIG_COND_RESET=0
    I2Cdev_writeByte(MPU6050_address, 0x37, 0x02);             //INT_PIN_CFG   -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=1 ; CLKOUT_EN=0
	//note: something seems to be wrong in the spec here. With AFS=2 1G = 4096 but according to my measurement: 1G=2048 (and 2048/8 = 256)
	//confirmed here: http://www.multiwii.com/forum/viewtopic.php?f=8&t=1080&start=10#p7480

	// cau hinh 5883
	HMC5883_Init(HMC5883L_ADDR, 0);

	//at this stage, the MAG is configured via the original MAG init function in I2C bypass mode
	//now we configure MPU as a I2C Master device to handle the MAG via the I2C AUX port (done here for HMC5883)
	I2Cdev_writeByte(MPU6050_address, 0x6A, 0b00100000);       //USER_CTRL     -- DMP_EN=0 ; FIFO_EN=0 ; I2C_MST_EN=1 (I2C master mode) ; I2C_IF_DIS=0 ; FIFO_RESET=0 ; I2C_MST_RESET=0 ; SIG_COND_RESET=0
	I2Cdev_writeByte(MPU6050_address, 0x37, 0x00);             //INT_PIN_CFG   -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=0 ; CLKOUT_EN=0
	I2Cdev_writeByte(MPU6050_address, 0x24, 0x0D);             //I2C_MST_CTRL  -- MULT_MST_EN=0 ; WAIT_FOR_ES=0 ; SLV_3_FIFO_EN=0 ; I2C_MST_P_NSR=0 ; I2C_MST_CLK=13 (I2C slave speed bus = 400kHz)
	I2Cdev_writeByte(MPU6050_address, 0x25, 0x80 | (HMC5883L_ADDR));//I2C_SLV0_ADDR -- I2C_SLV4_RW=1 (read operation) ; I2C_SLV4_ADDR=MAG_ADDRESS
	I2Cdev_writeByte(MPU6050_address, 0x26, MAG_DATA_REGISTER);//I2C_SLV0_REG  -- 6 data bytes of MAG are stored in 6 registers. First register address is MAG_DATA_REGISTER
	I2Cdev_writeByte(MPU6050_address, 0x27, 0x86);             //I2C_SLV0_CTRL -- I2C_SLV0_EN=1 ; I2C_SLV0_BYTE_SW=0 ; I2C_SLV0_REG_DIS=0 ; I2C_SLV0_GRP=0 ; I2C_SLV0_LEN=3 (3x2 bytes)
	#endif

	MPU6050_zeroCalibrate(64, 5);
}

void MPU6050_setClockSource(uint8_t source)
{
    I2Cdev_writeBits(MPU6050_address, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
}

void MPU6050_setFullScaleGyroRange(uint8_t range)
{
	I2Cdev_writeBits(MPU6050_address, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
}

void MPU6050_setFullScaleAccelRange(uint8_t range)
{
	I2Cdev_writeBits(MPU6050_address, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
}

void MPU6050_setSleepEnabled(unsigned char enabled)
{
    I2Cdev_writeBit(MPU6050_address, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
}

void MPU6050_zeroCalibrate(unsigned char totSamples, unsigned char sampleDelayMS)
{
	float x, y, z;
	gOffsets[0] = 0.0;
	gOffsets[1] = 0.0;
	gOffsets[2] = 0.0;
	for (unsigned char i = 0; i < totSamples; i++)
	{
		_delay_ms(sampleDelayMS);
		MPU6050_getRotation(&x, &y, &z);
		gOffsets[0] = (gOffsets[0] * i + x) / (i + 1);
		gOffsets[1] = (gOffsets[1] * i + y) / (i + 1);
		gOffsets[2] = (gOffsets[2] * i + z) / (i + 1);
	}
}

void MPU6050_getAcceleration(int16_t* x, int16_t* y, int16_t* z)
{
	I2Cdev_readBytes(MPU6050_address, MPU6050_RA_ACCEL_XOUT_H, 6, _6050buff);
	*x = (((int16_t)_6050buff[0]) << 8) | _6050buff[1];
	*y = (((int16_t)_6050buff[2]) << 8) | _6050buff[3];
	*z = (((int16_t)_6050buff[4]) << 8) | _6050buff[5];
}

void MPU6050_getRotation(float* x, float* y, float* z)
{
	I2Cdev_readBytes(MPU6050_address, MPU6050_RA_GYRO_XOUT_H, 6, _6050buff);
	*x = (float)((((int16_t)_6050buff[0]) << 8) | _6050buff[1]) * 1.0642251537E-3;	// rad/s - thang chia 250 deg/s
	*y = (float)((((int16_t)_6050buff[2]) << 8) | _6050buff[3]) * 1.0642251537E-3;
	*z = (float)((((int16_t)_6050buff[4]) << 8) | _6050buff[5]) * 1.0642251537E-3;
}

void MPU6050_getMotion9(float* ax, float* ay, float* az, float* gx, float* gy, float* gz, float* mx, float* my, float* mz)
{
	I2Cdev_readBytes(MPU6050_address, MPU6050_RA_ACCEL_XOUT_H, 20, _6050buff);
	*ax = (float)((((int16_t)_6050buff[0]) << 8) | _6050buff[1]) / 4.096;			// thang chia 2g
	*ay = (float)((((int16_t)_6050buff[2]) << 8) | _6050buff[3]) / 4.096;
	*az = (float)((((int16_t)_6050buff[4]) << 8) | _6050buff[5]) / 4.096;
	*gx = (float)((((int16_t)_6050buff[8]) << 8) | _6050buff[9]) * 1.0642251537E-3 - gOffsets[0];	// rad/s - thang chia 2000 deg/s
	*gy = (float)((((int16_t)_6050buff[10]) << 8) | _6050buff[11]) * 1.0642251537E-3 - gOffsets[1];
	*gz = (float)((((int16_t)_6050buff[12]) << 8) | _6050buff[13]) * 1.0642251537E-3 - gOffsets[2];
	*mx = (float)((((int16_t)_6050buff[14]) << 8) | _6050buff[15]);
	*mz = (float)((((int16_t)_6050buff[16]) << 8) | _6050buff[17]);
	*my = (float)((((int16_t)_6050buff[18]) << 8) | _6050buff[19]);
}

int8_t I2Cdev_readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data)//, uint16_t timeout)
{
	uint8_t b;
	uint8_t count = I2Cdev_readByte(devAddr, regAddr, &b);//, timeout);
	*data = b & (1 << bitNum);
	return count;
}

int8_t I2Cdev_readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data)//, uint16_t timeout)
{
	return I2Cdev_readBytes(devAddr, regAddr, 1, data);//, timeout);
}

int8_t I2Cdev_readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
{
//int8_t count = 0;
	i2c_start(devAddr + I2C_WRITE);
	i2c_write(regAddr);
	i2c_stop();

	i2c_start(devAddr + I2C_READ);
	uint8_t i = 0;
//	uint32_t t1 = millis();
	while (i < length - 1)// && (timeout == 0 || millis() - t1 < timeout))
	{
   		data[i++] = i2c_readAck();				// receive DATA
	}
	data[i] = i2c_readNak();					// receive DATA
//	if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout

	i2c_stop();									// end transmission
	return 0;//count;
}

unsigned char I2Cdev_writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data)
{
	uint8_t b;
	I2Cdev_readByte(devAddr, regAddr, &b);
	b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
	return I2Cdev_writeByte(devAddr, regAddr, b);
}

unsigned char I2Cdev_writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data)
{
    //      010 value to write
    // 76543210 bit numbers
    //    xxx   args: bitStart=4, length=3
    // 00011100 mask byte
    // 10101111 original value (sample)
    // 10100011 original & ~mask
    // 10101011 masked | value
	uint8_t b;
    if (I2Cdev_readByte(devAddr, regAddr, &b) != 0)
	{
		uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
		data <<= (bitStart - length + 1); // shift data into correct position
		data &= mask; // zero all non-important bits in data
		b &= ~(mask); // zero all important bits in existing byte
		b |= data; // combine data with existing byte
		return I2Cdev_writeByte(devAddr, regAddr, b);
    }
	else
	{
		return 0;
	}
}

unsigned char I2Cdev_writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)
{
	return I2Cdev_writeBytes(devAddr, regAddr, 1, &data);
}

unsigned char I2Cdev_writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data)
{
//	uint8_t status = 0;
	i2c_start(devAddr + I2C_WRITE);
	i2c_write(regAddr);
	for (uint8_t i = 0; i < length; i++)
	{
		i2c_write(data[i]);
	}
	i2c_stop();
    return 0;//status == 0;
}

#endif
/*-------------------------------------------------------*/
// Magnetometer functions
/*-------------------------------------------------------*/

#if defined(HMC5843) || defined(HMC5883)
void HMC5883_Init(unsigned char adr, unsigned char setmode /*0 - init*/)
{
	HMC5883_address = adr << 1;

	_delay_ms(5);
	if (setmode) HMC5883_setMode(0);

	HMC5883_writeReg(HMC5883_R_CONFA, 0x70);
	HMC5883_writeReg(HMC5883_R_CONFB, 0xA0);
	HMC5883_writeReg(HMC5883_R_MODE, 0x00);

	// Calibrate HMC using self test, not recommended to change the gain after calibration.
	HMC5883_calibrate(1); // Use gain 1=default, valid 0-7, 7 not recommended.
	// Single mode conversion was used in calibration, now set continuous mode
	HMC5883_setMode(0);
	_delay_ms(10);
	HMC5883_setDOR(6);
}

void HMC5883_calibrate(unsigned char gain)
{
	x_scale=1; // get actual values
	y_scale=1;
	z_scale=1;
	HMC5883_writeReg(HMC5883_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR=0x010 + MS1,MS0 set to pos bias
	HMC5883_setGain(gain);
	float x, y, z, mx = 0, my = 0, mz = 0;
	for (unsigned char i = 0; i < 10; i++)
	{
		HMC5883_setMode(1);
		HMC5883_getValues(&x, &y, &z);
		if (x > mx) mx = x;
		if (y > my) my = y;
		if (z > mz) mz = z;
	}
	float max = 0;
	if (mx > max) max = mx;
	if (my > max) max = my;
	if (mz > max) max = mz;
	x_max = mx;
	y_max = my;
	z_max = mz;
	x_scale = max / mx; // calc. scales
	y_scale = max / my;
	z_scale = max / mz;
	HMC5883_writeReg(HMC5883_R_CONFA, 0x010); // set RegA/DOR back to default
}

void HMC5883_setMode(unsigned char mode)
{
	if (mode > 2) return;
	HMC5883_writeReg(HMC5883_R_MODE, mode);
	_delay_ms(100);
}

// set data output rate
// 0-6, 4 default, normal operation assumed
void HMC5883_setDOR(unsigned char DOR)
{
	if (DOR > 6) return;
	HMC5883_writeReg(HMC5883_R_CONFA, DOR << 2);
}

void HMC5883_setGain(unsigned char gain)
{// 0-7, 1 default
	if (gain > 7) return;
	HMC5883_writeReg(HMC5883_R_CONFB, gain << 5);
}

void HMC5883_writeReg(unsigned char reg, unsigned char val)
{
	i2c_start(HMC5883_address + I2C_WRITE);
	i2c_write(reg);							// send register address
	i2c_write(val);							// send value to write
	i2c_stop();								// end transmission
}

void HMC5883_getRaw(int *x, int *y, int *z)
{
	i2c_start(HMC5883_address + I2C_WRITE);
	i2c_write(HMC5883_R_XM); // will start from DATA X MSB and fetch all the others
	i2c_stop();
	i2c_start(HMC5883_address + I2C_READ);
	// read out the 3 values, 2 bytes each.
    *x = (i2c_readAck() << 8) | i2c_readAck();
	#ifdef HMC5843
	*y = (i2c_readAck() << 8) | i2c_readAck();
	*z = (i2c_readAck() << 8) | i2c_readNak();
	#endif
	#ifdef HMC5883 // the Z registers comes before the Y registers in the HMC5883L
	*z = (i2c_readAck() << 8) | i2c_readAck();
	*y = (i2c_readAck() << 8) | i2c_readNak();
    #endif
    // the HMC5883 will automatically wrap around on the next request
	i2c_stop();
}

void HMC5883_getValues(float *x, float *y, float *z)
{
	int xr, yr, zr;
	HMC5883_getRaw(&xr, &yr, &zr);
	*x = (float)(xr / x_scale);
	*y = (float)(yr / y_scale);
	*z = (float)(zr / z_scale);
}
#endif

/*-------------------------------------------------------*/
// Barometer functions
/*-------------------------------------------------------*/

#if (defined MS5607 || defined MS5611)

void MS56XX_writeReg(unsigned char reg, unsigned char val)
{
	i2c_start(MS56XX_address + I2C_WRITE);
	i2c_write(reg);							// send register address
	i2c_write(val);							// send value to write
	i2c_stop();								// end transmission
}

void MS56XX_Reset()
{
	MS56XX_writeReg(MS561101BA_RESET, 0);
}

void MS56XX_readCalibration()
{
	union {uint16_t val; uint8_t raw[2];} data;
	for(uint8_t i = 0; i < 6; i++)
	{
		i2c_start(MS56XX_address + 0);
		i2c_write(0xA2 + 2 * i);
		i2c_stop();
		_delay_ms(10);
		i2c_start(MS56XX_address + 1);		// I2C read direction => 1
		_delay_ms(10);
		data.raw[1] = i2c_readAck();		// read a 16 bit register
		data.raw[0] = i2c_readNak();
		ms561101ba_ctx.c[i+1] = data.val;
		i2c_stop();
	}
}

void MS56XX_Init(unsigned char adr)
{
	MS56XX_address = adr << 1;
	_delay_ms(10);
	MS56XX_Reset();
	_delay_ms(100);
	MS56XX_readCalibration();
}

void MS56XX_UT_Start()
{
  i2c_start(MS56XX_address + 0);			// I2C write direction
  i2c_write(MS561101BA_TEMPERATURE + OSR);	// register selection
  i2c_stop();
}

// read uncompensated pressure value: send command first
void MS56XX_UP_Start()
{
  i2c_start(MS56XX_address + 0);			// I2C write direction
  i2c_write(MS561101BA_PRESSURE + OSR);		// register selection
  i2c_stop();
}

void MS56XX_UP_Read()
{
	i2c_start(MS56XX_address + 0);
	i2c_write(0);
	i2c_start(MS56XX_address + 1);
	ms561101ba_ctx.up.raw[2] = i2c_readAck();
	ms561101ba_ctx.up.raw[1] = i2c_readAck();
	ms561101ba_ctx.up.raw[0] = i2c_readNak();
}

void MS56XX_UT_Read()
{
	i2c_start(MS56XX_address + 0);
	i2c_write(0);
	i2c_start(MS56XX_address + 1);
	ms561101ba_ctx.ut.raw[2] = i2c_readAck();
	ms561101ba_ctx.ut.raw[1] = i2c_readAck();
	ms561101ba_ctx.ut.raw[0] = i2c_readNak();
}

void MS56XX_Calculate()
{
	int32_t temperature, off2 = 0, sens2 = 0, delt;

	int32_t dT   = ms561101ba_ctx.ut.val - ((uint32_t)ms561101ba_ctx.c[5] << 8);
	int64_t off  = ((uint32_t)ms561101ba_ctx.c[2] <<16) + (((int64_t)dT * ms561101ba_ctx.c[4]) >> 7);
	int64_t sens = ((uint32_t)ms561101ba_ctx.c[1] <<15) + (((int64_t)dT * ms561101ba_ctx.c[3]) >> 8);
	temperature  = 2000 + (((int64_t)dT * ms561101ba_ctx.c[6])>>23);

	if (temperature < 2000)
	{ // temperature lower than 20st.C 
		delt = temperature-2000;
		delt  = delt*delt;
		off2  = (5 * delt)>>1; 
		sens2 = (5 * delt)>>2; 
		if (temperature < -1500)
		{ // temperature lower than -15st.C
			delt  = temperature+1500;
			delt  = delt*delt;
			off2  += 7 * delt; 
			sens2 += (11 * delt)>>1; 
		}
	} 
	off  -= off2; 
	sens -= sens2;
	pressure = (( (ms561101ba_ctx.up.val * sens ) >> 21) - off) >> 15;
}

void Baro_update()
{
	MS56XX_UT_Start();
	_delay_ms(10);
	MS56XX_UT_Read(); 
	_delay_ms(10);
	MS56XX_UP_Start(); 
    _delay_ms(10);
	MS56XX_UP_Read();
	MS56XX_Calculate();
	BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 4433000.0f; //centimeter
}

/*
void MS56XX_Init(unsigned char adr)
{
	MS56XX_address = adr << 1;
	MS56XX_writeCmd(CMD_RESET);				// RESET command
	_delay_ms(3);							// wait for the reset sequence timing
	MS56XX_ReadCoefficients();
	startHigh = MS56XX_GetSampleHigh(SAMPLE_VALUE);	// tinh toan do cao ban dau
	currentHigh = startHigh;
}

void MS56XX_writeCmd(unsigned char cmd)
{
	i2c_start(MS56XX_address + I2C_WRITE);
	i2c_write(cmd);							// send command address
	i2c_stop();								// end transmission
}

void MS56XX_ReadCoefficients(void)
{
	uint8_t i, c;

	for (i = 0; i < 8; i++)
	{
		MS56XX_writeCmd(CMD_PROM_RD + i * 2);	// send PROM READ command
		i2c_start(MS56XX_address + I2C_READ);	// read 0xEF
		c = i2c_readAck();						// read MSB and acknowledge
		C[i] = 256 * c;
		c = i2c_readNak();						// read LSB and not acknowledge
		C[i] = C[i] + c;
		i2c_stop();
	}
}

unsigned long MS56XX_Cmd_ADC(char cmd)
{
	unsigned int ret;
	unsigned long temp=0;
	MS56XX_writeCmd(CMD_ADC_CONV + cmd);		// send conversion command
	switch (cmd & 0x0f)							// wait necessary conversion time
	{
		case CMD_ADC_256 : _delay_us(900); break;
		case CMD_ADC_512 : _delay_ms(3); break;
		case CMD_ADC_1024: _delay_ms(4); break;
		case CMD_ADC_2048: _delay_ms(6); break;
		case CMD_ADC_4096: _delay_ms(10); break;
	}
	MS56XX_writeCmd(CMD_ADC_READ);				// send conversion command

	i2c_start(MS56XX_address + I2C_READ);
	ret = i2c_readAck();						// read HSB and acknowledge
	temp = 65536 * ret;
	ret = i2c_readAck();						// read MSB and acknowledge
	temp = temp + 256 * ret;
	ret = i2c_readNak();						// read LSB and acknowledge
	temp = temp + ret;
	i2c_stop();
	return temp;
}

double MS56XX_GetPressure(void)
{
	double P;						// compensated pressure value
	double T;						// compensated temperature value
	double dT;						// difference between actual and measured temperature
	double OFF;						// offset at actual temperature
	double SENS;					// sensitivity at actual temperature
	unsigned long D11;				// ADC value of the pressure conversion
	unsigned long D22;				// ADC value of the temperature conversion

	D22 = MS56XX_Cmd_ADC(CMD_ADC_D2 + CMD_ADC_4096); // read D2
	D11 = MS56XX_Cmd_ADC(CMD_ADC_D1 + CMD_ADC_4096); // read D1

	// calculate 1st order pressure and temperature (MS5607 1st order algorithm)
	dT = D22 - C[5] * pow(2, 8);
	OFF = C[2] * pow(2, 17) + dT * C[4] / pow(2, 6);
	SENS = C[1] * pow(2, 16) + dT * C[3] / pow(2, 7);
	T = (2000 + (dT * C[6]) / pow(2, 23));
	P = (((D11 * SENS) / pow(2, 21) - OFF) / pow(2, 15)) / 100;	// pascal
	return P;
}

double MS56XX_GetHigh()
{
	float getHigh = (1 - pow((MS56XX_GetPressure() / 1013.25), 0.190284)) * 145366.45 * 0.3048;	// * 0.3048 chuyen tu feet sang met
	//currentHigh = (currentHigh * SMOOTH_SCALE + getHigh) / (SMOOTH_SCALE + 1);
	return getHigh;
}

double MS56XX_GetSampleHigh(unsigned char samples)
{
	float H = 0.0;
	for (unsigned char i = 0; i < samples; i++)
	{
		H += MS56XX_GetPressure();
		_delay_ms(70);
	}
	H = (float)(H / samples);
	H = (1 - pow((H / 1013.25), 0.190284)) * 145366.45 * 0.3048;	// * 0.3048 chuyen tu feet sang met
	return H;
}
*/

#endif

//fdef BMP085
/*
void BMP085_Init(unsigned char adr)
{
	BMP085_address = adr << 1;		// BMP085_ADDR;
	_pressure_waittime[0] = 4500;	// These are maximum convertion times.
	_pressure_waittime[1] = 7500;	// It is possible to use pin EOC (End Of Conversion)
	_pressure_waittime[2] = 13500;	// to check if conversion is finished (logic 1) 
	_pressure_waittime[3] = 25500;	// or running (logic 0) insted of waiting for convertion times.
	_cm_Offset = 0;
	_Pa_Offset = 0;					// 1hPa = 100Pa = 1mbar
	BMP085_Init_Param(MODE_STANDARD, 0, 1);
	//BMP085_Init_Param(MODE_ULTRA_HIGHRES, 0, 1);
}

void BMP085_Init_Param(unsigned char _BMPMode, int32_t _initVal, unsigned char _Unitmeters)
{
	BMP085_GetCalData();			// initialize cal data
	BMP085_CalcTrueTemperature();	// initialize b5
	BMP085_SetMode(_BMPMode);
	_Unitmeters ? BMP085_SetLocalAbsAlt(_initVal) : BMP085_SetLocalPressure(_initVal); 
}

void BMP085_Writemem(uint8_t _addr, uint8_t _val)
{
	i2c_start(BMP085_address + I2C_WRITE);	// start transmission to device
	i2c_write(_addr);				// sends register address to read from
	i2c_write(_val);				// send value to write
	i2c_stop();						// end transmission
}

void BMP085_Readmem(uint8_t _addr, uint8_t _nbytes, uint8_t __buff[])
{
	i2c_start(BMP085_address + I2C_WRITE);	// start transmission to device
	i2c_write(_addr);				// sends register address to read from
	i2c_stop();						// end transmission

	i2c_start(BMP085_address + I2C_READ);	// start transmission to device

	uint8_t i = 0;
	while (i < _nbytes - 1)
	{
    	__buff[i++] = i2c_readAck();// receive DATA
	}
	__buff[i] = i2c_readNak();		// receive DATA
	i2c_stop();						// end transmission
}

void BMP085_GetCalData()
{
	BMP085_Readmem(CAL_AC1, 2, _bbuff);  ac1 = ((int)_bbuff[0] <<8 | ((int)_bbuff[1]));
	BMP085_Readmem(CAL_AC2, 2, _bbuff);  ac2 = ((int)_bbuff[0] <<8 | ((int)_bbuff[1]));
	BMP085_Readmem(CAL_AC3, 2, _bbuff);  ac3 = ((int)_bbuff[0] <<8 | ((int)_bbuff[1]));
	BMP085_Readmem(CAL_AC4, 2, _bbuff);  ac4 = ((unsigned int)_bbuff[0] <<8 | ((unsigned int)_bbuff[1]));
	BMP085_Readmem(CAL_AC5, 2, _bbuff);  ac5 = ((unsigned int)_bbuff[0] <<8 | ((unsigned int)_bbuff[1]));
	BMP085_Readmem(CAL_AC6, 2, _bbuff);  ac6 = ((unsigned int)_bbuff[0] <<8 | ((unsigned int)_bbuff[1]));
	BMP085_Readmem(CAL_B1, 2, _bbuff);   b1 = ((int)_bbuff[0] <<8 | ((int)_bbuff[1]));
	BMP085_Readmem(CAL_B2, 2, _bbuff);   b2 = ((int)_bbuff[0] <<8 | ((int)_bbuff[1]));
	BMP085_Readmem(CAL_MB, 2, _bbuff);   mb = ((int)_bbuff[0] <<8 | ((int)_bbuff[1]));
	BMP085_Readmem(CAL_MC, 2, _bbuff);   mc = ((int)_bbuff[0] <<8 | ((int)_bbuff[1]));
	BMP085_Readmem(CAL_MD, 2, _bbuff);   md = ((int)_bbuff[0] <<8 | ((int)_bbuff[1]));
}

void BMP085_CalcTrueTemperature()
{
	long ut,x1,x2;

	//read Raw Temperature
	BMP085_Writemem(CONTROL, READ_TEMPERATURE);
	_delay_us(4500);						// min. 4.5us read Temp delay
	BMP085_Readmem(CONTROL_OUTPUT, 2, _bbuff);
	ut = ((long)_bbuff[0] << 8 | (long)_bbuff[1]);		// uncompensated temperature value

	// calculate temperature
	x1 = ((long)ut - ac6) * ac5 >> 15;
	x2 = ((long)mc << 11) / (x1 + md);
	b5 = x1 + x2;
}

void BMP085_SetMode(unsigned char _BMPMode)
{
	_oss = _BMPMode;
}

void BMP085_GetPressure(int32_t *_Pa)
{
	long TruePressure;
	BMP085_CalcTruePressure(&TruePressure);
	*_Pa = TruePressure / pow((1 - (float)_param_centimeters / 4433000), 5.255) + _Pa_Offset;
}

void BMP085_CalcTruePressure(long *_TruePressure)
{
	long up, x1, x2, x3, b3, b6, p;
	unsigned long b4, b7;
	int32_t tmp; 

	#if AUTO_UPDATE_TEMPERATURE
	BMP085_CalcTrueTemperature();        // b5 update 
	#endif 

	//read Raw Pressure
	BMP085_Writemem(CONTROL, READ_PRESSURE+(_oss << 6));
	_delay_us(_pressure_waittime[_oss]);    
	BMP085_Readmem(CONTROL_OUTPUT, 3, _bbuff);  
	up = ((((long)_bbuff[0] << 16) | ((long)_bbuff[1] <<8) | ((long)_bbuff[2])) >> (8 - _oss)); // uncompensated pressure value

	// calculate true pressure
	b6 = b5 - 4000;             // b5 is updated by calcTrueTemperature().
	x1 = (b2* (b6 * b6 >> 12)) >> 11;
	x2 = ac2 * b6 >> 11;
	x3 = x1 + x2;
	tmp = ac1;
	tmp = (tmp * 4 + x3) << _oss;
	b3 = (tmp + 2) >> 2;
	x1 = ac3 * b6 >> 13;
	x2 = (b1 * (b6 * b6 >> 12)) >> 16;
	x3 = ((x1 + x2) + 2) >> 2;
	b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15;
	b7 = ((uint32_t)up - b3) * (50000 >> _oss);
	p = b7 < 0x80000000 ? (b7 << 1) / b4 : (b7 / b4) << 1;
	x1 = (p >> 8) * (p >> 8);
	x1 = (x1 * 3038) >> 16;
	x2 = (-7357 * p) >> 16;
	*_TruePressure = p + ((x1 + x2 + 3791) >> 4);
}

void BMP085_SetLocalAbsAlt(int32_t _centimeters)
{
	int32_t tmp_Pa;
	_param_centimeters = _centimeters;   
	BMP085_GetPressure(&tmp_Pa);			// calc pressure based on current altitude
	_param_datum = tmp_Pa;
}

void BMP085_SetLocalPressure(int32_t _Pa)
{
	int32_t tmp_alt;
 	 _param_datum = _Pa;   
 	BMP085_GetAltitude(&tmp_alt);    // calc altitude based on current pressure   
	_param_centimeters = tmp_alt;
}

void BMP085_GetAltitude(int32_t *_centimeters)
{
	long TruePressure;
	BMP085_CalcTruePressure(&TruePressure);
	*_centimeters =  4433000 * (1 - pow((TruePressure / (float)_param_datum), 0.190295)) + _cm_Offset;
}
*/

static struct
{
	// sensor registers from the BOSCH BMP085 datasheet
	int16_t  ac1, ac2, ac3, b1, b2, mb, mc, md;
	uint16_t ac4, ac5, ac6;
	union {uint16_t val; uint8_t raw[2];} ut; //uncompensated T
	union {uint32_t val; uint8_t raw[4];} up; //uncompensated P
	uint8_t  state;
	uint32_t deadline;
} bmp085_ctx; 

uint8_t BMP085_address;

int16_t BMP085_readIntRegister(uint8_t r)
{
	union {int16_t val; uint8_t raw[2];} data;
	i2c_start(BMP085_address + I2C_WRITE);	// start transmission to device
	i2c_write(r);					// sends register address to read from
	i2c_stop();						// end transmission
	i2c_start(BMP085_address + I2C_READ);	// start transmission to device
	data.raw[1] = i2c_readAck();
	data.raw[0] = i2c_readNak();
	return data.val;
}

void BMP085_readCalibration()
{
	_delay_ms(10);
	bmp085_ctx.ac1 = BMP085_readIntRegister(0xAA);
	bmp085_ctx.ac2 = BMP085_readIntRegister(0xAC);
	bmp085_ctx.ac3 = BMP085_readIntRegister(0xAE);
	bmp085_ctx.ac4 = BMP085_readIntRegister(0xB0);
	bmp085_ctx.ac5 = BMP085_readIntRegister(0xB2);
	bmp085_ctx.ac6 = BMP085_readIntRegister(0xB4);
	bmp085_ctx.b1  = BMP085_readIntRegister(0xB6);
	bmp085_ctx.b2  = BMP085_readIntRegister(0xB8);
	bmp085_ctx.mb  = BMP085_readIntRegister(0xBA);
	bmp085_ctx.mc  = BMP085_readIntRegister(0xBC);
	bmp085_ctx.md  = BMP085_readIntRegister(0xBE);
}

// read uncompensated temperature value: send command first
void BMP085_UT_Start()
{
	i2c_start(BMP085_address + I2C_WRITE);	// start transmission to device
	i2c_write(0xf4);					// sends register address to read from
	i2c_write(0x2e);					// sends register address to read from
	i2c_stop();						// end transmission
	i2c_start(BMP085_address + I2C_WRITE);	// start transmission to device
	i2c_write(0xf6);					// sends register address to read from
	i2c_stop();
}

// read uncompensated temperature value: read result bytes
// the datasheet suggests a delay of 4.5 ms after the send command
void BMP085_UT_Read()
{
	i2c_start(BMP085_address + I2C_READ);//I2C read direction => 1
	bmp085_ctx.ut.raw[1] = i2c_readAck();
	bmp085_ctx.ut.raw[0] = i2c_readNak();
	i2c_stop();
}

//in a whole cycle: we read temperature one time and pressure 5 times
//in a whole cycle: we read temperature one time and pressure 5 times
//in a whole cycle: we read temperature one time and pressure 5 times
//in a whole cycle: we read temperature one time and pressure 5 times
void BMP085_Init(unsigned char adr)
{
	BMP085_address = adr << 1;		// BMP085_ADDR;
	_delay_ms(10);
	BMP085_readCalibration();
	BMP085_UT_Start(); 
	_delay_ms(5);
	BMP085_UT_Read();
}

//ndif
